import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, GroupAction, LogInfo,
                            IncludeLaunchDescription, SetEnvironmentVariable)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node, PushRosNamespace

def generate_launch_description():
    # 获取参数
    use_sim_time = LaunchConfiguration('use_sim_time')

    # 打印参数
    print_use_sim_time = LogInfo(msg=['use_sim_time: ', use_sim_time])

    # 定义启动组
    nav_group = GroupAction([
        # imu互补滤波波
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('imu_complementary_filter'), 'launch', "complementary_filter.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                }.items()),

        # lidar_2d_undistortion
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('sensors_preprocess'), 'launch', "lidar_2d_undistortion.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                                'default_robot_parameter_file': LaunchConfiguration('default_robot_parameter_file'),
                                }.items()),

        # 传感器数据处理
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('sensors_preprocess'), 'launch', "sensors_preprocess.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                                'default_robot_parameter_file': LaunchConfiguration('default_robot_parameter_file'),
                                'default_maps_parameter_file': LaunchConfiguration('default_maps_parameter_file'),
                                }.items()),
                                
        #slamkware_ros_bridge
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('slamware_ros_bridge'), 'launch', "slamware_ros_bridge.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                                'default_maps_parameter_file': LaunchConfiguration('default_maps_parameter_file'),
                                }.items()),
        #slamkware_ros_sdk
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('slamware_ros_sdk'), 'launch', "slamware_ros_sdk_server_node.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                                'default_maps_parameter_file': LaunchConfiguration('default_maps_parameter_file'),
                                }.items()),
        
        # 地图管理相关节点
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('maps_manager'), 'launch', "maps_manager.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                                'default_maps_parameter_file': LaunchConfiguration('default_maps_parameter_file'),
                                }.items()),

        # costmap_2d
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('costmap_2d'), 'launch', "costmap_2d.launch.py")),
            launch_arguments={'use_sim_time': LaunchConfiguration('use_sim_time'),
                                'global_parameter_file': LaunchConfiguration('global_parameter_file'),
                                'default_robot_parameter_file': LaunchConfiguration('default_robot_parameter_file'),
                                'default_maps_parameter_file': LaunchConfiguration('default_maps_parameter_file'),
                                }.items()),
    ])

    ld = LaunchDescription([
        print_use_sim_time,
        nav_group
    ])
    return ld